//--- Projeto Domus
// interactionsource.com
// Koji Pereira, Andre Veloso e Marcos Paulo

/*
 Projeto Domus
 interactionsource.com
 Koji Pereira, Andre Veloso e Marcos Paulo

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/


package com.interactionsource.input.wiimoteosc;
import org.wiigee.device.Wiimote;
import org.wiigee.util.Log;

import com.illposed.osc.OSCListener;
import com.illposed.osc.OSCMessage;
import com.illposed.osc.OSCPortIn;
import com.interactionsource.event.ContinuousEvent;
import com.interactionsource.event.ContinuousEventListener;
import com.interactionsource.event.TriggerEvent;
import com.interactionsource.event.TriggerEventListener;

import java.lang.Math;

public class WiimoteOSCStreamer implements OSCListener{

	private WiimoteOSC wiimote;
	private OSCPortIn receiverCon;

	protected TriggerEventListener pulseTriggerEventListener;
	protected ContinuousEventListener globalForceContinuousEventListener;

	//Threshold de deteccao de picos e vales, para detecao de batidas curtas
	public static final float THRESHOLD_PEAK = 0.7f;

	private float x0, y0, z0 = 0.0F;
	private float psi0, theta0, phi0 = 0.0F;
	private float yaw, pitch, roll = 0.0F;

	private double resultante = 0.0d;

	//Variaveis usadas para analise de amostragem temporal
	private float  x1, y1, z1 = 0.0F;
	private float  x2, y2, z2 = 0.0F;
	private float  x3, y3, z3 = 0.0F;
	private float  x4, y4, z4 = 0.0F;
	private float  diffX1, diffX2, diffX3, diffX4 = 0.0F;
	private float  diffY1, diffY2, diffY3, diffY4 = 0.0F;
	private float  diffZ1, diffZ2, diffZ3, diffZ4 = 0.0F;

	boolean isTriggerPeak = false;
	boolean isTriggerPeakOld = false;

	private int testeVal;

	private int cont = 0;

	protected static final String OSC_GX = "/wiimotePD/gX";
	protected static final String OSC_GY = "/wiimotePD/gY";
	protected static final String OSC_GZ = "/wiimotePD/gZ";

	protected static final String OSC_YawSpeed   = "/wiimotePD/YawSpeed";
	protected static final String OSC_PitchSpeed = "/wiimotePD/PitchSpeed";
	protected static final String OSC_RollSpeed  = "/wiimotePD/RollSpeed";

	protected static final String OSC_ButtonA = "/wiimotePD/A";
	protected static final String OSC_ButtonB = "/wiimotePD/B";

	protected static final String OSC_ButtonHome = "/wiimotePD/Home";
	protected static final String OSC_ButtonMinus = "/wiimotePD/Minus";
	protected static final String OSC_ButtonPlus = "/wiimotePD/Plus";

	protected static final String OSC_ButtonOne = "/wiimotePD/One";    
	protected static final String OSC_ButtonTwo = "/wiimotePD/Two";

	protected static final String OSC_ButtonUp = "/wiimotePD/Up";
	protected static final String OSC_ButtonDown = "/wiimotePD/Down";
	protected static final String OSC_ButtonLeft = "/wiimotePD/Left";    
	protected static final String OSC_ButtonRight = "/wiimotePD/Right";

	protected WiimoteOSCStreamer(WiimoteOSC wiimote) {
		this.wiimote = wiimote;
		this.receiverCon = wiimote.getReceiveConnection();

		receiverCon.addListener(OSC_GX, this);
		receiverCon.addListener(OSC_GY, this);
		receiverCon.addListener(OSC_GZ, this);

		receiverCon.addListener(OSC_YawSpeed, this);
		receiverCon.addListener(OSC_PitchSpeed, this);
		receiverCon.addListener(OSC_RollSpeed, this);

		receiverCon.addListener(OSC_ButtonA, this);
		receiverCon.addListener(OSC_ButtonB, this);

		receiverCon.addListener(OSC_ButtonHome, this);
		receiverCon.addListener(OSC_ButtonMinus, this);
		receiverCon.addListener(OSC_ButtonPlus, this);

		receiverCon.addListener(OSC_ButtonOne, this);
		receiverCon.addListener(OSC_ButtonTwo, this);

		receiverCon.addListener(OSC_ButtonUp, this);
		receiverCon.addListener(OSC_ButtonDown, this);
		receiverCon.addListener(OSC_ButtonLeft, this);
		receiverCon.addListener(OSC_ButtonRight, this);


		receiverCon.startListening();	        

		Log.write("WiimoteOSCStreamer initialized... Recebendo dados via OSC");
	}	

	public void acceptMessage(java.util.Date time, OSCMessage message) {		

		//System.out.println("WiimoteOSCStreamer- " + message.getAddress() + " " + message.getArguments()[0].toString());

		boolean isSensorData = false;

		//Sensor data
		if ( message.getAddress().equals(OSC_GX) ) {	


			//Guarda as variaveis para analise temporal
			x4 = x3;
			x3 = x2;
			x2 = x1;
			x1 = x0;


			x0 = (float) Integer.valueOf(message.getArguments()[0].toString()) ;
			x0 = (x0/100);
			isSensorData = true;

			diffX1 = Math.abs(x0 - x1);
			diffX2 = Math.abs(x0 - x2);
			diffX3 = Math.abs(x0 - x3);
			diffX4 = Math.abs(x0 - x4);

			//System.out.println("Diff -  diffX1:" + diffX1 + " diffX2:" + diffX2 +
			//		           " diffX3:" + diffX3 + " diffX4:" + diffX4);
			//System.out.println( " diffX4:" + diffX4);


		}

		if ( message.getAddress().equals(OSC_GY) ) {

			//Guarda as variaveis para analise temporal
			y4 = y3;
			y3 = y2;
			y1 = y0;

			y0 = (float) Integer.valueOf(message.getArguments()[0].toString()) ;
			y0 = (y0 /100) ;
			isSensorData = true;

			diffY1 = Math.abs(y0 - y1);
			diffY2 = Math.abs(y0 - y2);
			diffY3 = Math.abs(y0 - y3);
			diffY4 = Math.abs(y0 - y4);


		}

		if ( message.getAddress().equals(OSC_GZ) ) {

			//Guarda as variaveis para analise temporal
			z4 = z3;
			z3 = z2;
			z1 = z0;

			z0 =(float) Integer.valueOf(message.getArguments()[0].toString()) ;
			z0 = (z0 /100);
			isSensorData = true;

			diffZ1 = Math.abs(z0 - z1);
			diffZ2 = Math.abs(z0 - z2);
			diffZ3 = Math.abs(z0 - z3);
			diffZ4 = Math.abs(z0 - z4);

		}		

		if ( message.getAddress().equals(OSC_YawSpeed) ) {
			yaw = (float) Integer.valueOf(message.getArguments()[0].toString()) ;
			yaw = (yaw /100);
		}		

		if ( message.getAddress().equals(OSC_PitchSpeed) ) {
			pitch = (float) Integer.valueOf(message.getArguments()[0].toString()) ;
			pitch = (pitch /100) ;
		}		

		if ( message.getAddress().equals(OSC_RollSpeed) ) {
			roll = (float) Integer.valueOf(message.getArguments()[0].toString()) ;
			roll = (roll /100) ;
		}

		if (isSensorData) {

			this.handleAccelerationData(message);
			return;
		}

		//button data
		if ( message.getAddress().equals(OSC_ButtonA) ) {
			this.handleButtonData(message, Wiimote.BUTTON_A);
			return;
		}		

		if ( message.getAddress().equals(OSC_ButtonB) ) {
			this.handleButtonData(message, Wiimote.BUTTON_B);
			return;
		}

		if ( message.getAddress().equals(OSC_ButtonHome) ) {
			this.handleButtonData(message, Wiimote.BUTTON_HOME);
			return;
		}		

		if ( message.getAddress().equals(OSC_ButtonPlus) ) {
			this.handleButtonData(message, Wiimote.BUTTON_PLUS);
			return;
		}	

		if ( message.getAddress().equals(OSC_ButtonMinus) ) {
			this.handleButtonData(message, Wiimote.BUTTON_MINUS);
			return;
		}	

		if ( message.getAddress().equals(OSC_ButtonOne) ) {
			this.handleButtonData(message, Wiimote.BUTTON_1);
			return;
		}		

		if ( message.getAddress().equals(OSC_ButtonTwo) ) {
			this.handleButtonData(message, Wiimote.BUTTON_2);
			return;
		}		

		if ( message.getAddress().equals(OSC_ButtonUp) ) {
			this.handleButtonData(message, Wiimote.BUTTON_UP);
			return;
		}

		if ( message.getAddress().equals(OSC_ButtonDown) ) {
			this.handleButtonData(message, Wiimote.BUTTON_DOWN);
			return;
		}		

		if ( message.getAddress().equals(OSC_ButtonLeft) ) {
			this.handleButtonData(message, Wiimote.BUTTON_LEFT);
			return;
		}		

		if ( message.getAddress().equals(OSC_ButtonRight) ) {
			this.handleButtonData(message, Wiimote.BUTTON_RIGHT);
			return ;
		}		


		//System.out.println(" Yaw:" + yaw + " Pitch:" + pitch + " Roll:" + roll);

		return;
	}    

	/**
	 * Handles the retrieved acceleration data and fires
	 * corresponding Acceleration Event on the Device.
	 * 
	 * generate aditinal values for Global force and Peak gestures
	 * based on peak detection
	 * 
	 * @param data
	 */
	private void handleAccelerationData(OSCMessage message) {


		double x = (double) (x0);
		double y = (double) (y0);
		double z = (double) (z0);

		
		//Calcula a forca resultante
		this.resultante = (x0*x0) + (y0*y0) + (z0*z0) ;

		this.resultante = Math.sqrt( this.resultante );

		//Envia o evento da resultante para os listeners
		if (globalForceContinuousEventListener != null) {
			ContinuousEvent q = new ContinuousEvent(this, (float)resultante);
			this.globalForceContinuousEventListener.continuousEventReceived(q);
		}


		//System.out.println("resultante: " + resultante); 


		//if ( (x0>0.8) || (y0>0.8)  || (z0>0.8) || (x0<-0.8) || (y0<-0.8)  || (z0<-0.8) ) {
		//	System.out.println("Pico -  X:" + x0 + " Y:" + y0 + " Z:" + z0);
		//}

		
		//Realiza a deteccao de picos, para a deteccao e batidas
		//A deteccao de picos baseia-se na diferenca entre 4 samples consecutivos
		if ( (diffX1-diffX4>THRESHOLD_PEAK) || 
				(diffY1-diffY4>THRESHOLD_PEAK)  || 
				(diffZ1-diffZ4>THRESHOLD_PEAK) ) 
		{

			isTriggerPeak= true;

			if (isTriggerPeakOld) 
				isTriggerPeak= false;	

		} else {

			isTriggerPeakOld = false;

			isTriggerPeak= false;
		}

		if (isTriggerPeak) {
			System.out.println("Pico -  diffX1:" + diffX1 + " diffY1:" + diffY1 + " diffZ1:" + diffZ1);
			isTriggerPeakOld = true;

			//Envia evento para os listeners
			if (pulseTriggerEventListener != null) {
				TriggerEvent w = new TriggerEvent(this, 1);
				this.pulseTriggerEventListener.triggerEventReceived(w);
			}

		}


		//System.out.println("WiimoteOSCStreamer -  X:" + x + " Y:" + y + " Z:" + z);

		this.wiimote.fireAccelerationEvent(new double[]{x, y, z});

	}

	//novos listeners para os novos gesto implementados
	public void addPulseListener(TriggerEventListener listener){
		this.pulseTriggerEventListener = listener;
		//System.out.println("WiimoteOSCStreamer - pulseTriggerEventListener adcionado");
	}

	public void addGlobalForceListener(ContinuousEventListener listener) {
		this.globalForceContinuousEventListener = listener;
		//System.out.println("WiimoteOSCStreamer - globalForceContinuousEventListener adcionado");

	}

	private void handleButtonData(OSCMessage message, int button) {
		if (message.getArguments()[0].equals(1))
			this.wiimote.fireButtonPressedEvent(button);

		if (message.getArguments()[0].equals(0))
			this.wiimote.fireButtonReleasedEvent(button);   	

	}

	public void printValues() {
		System.out.println("X:" + x0 + " Y:" + y0 + " Z:" + z0 +
				" Yaw:" + yaw + " Pitch:" + pitch + " Roll:" + roll);
	}

	public void simulateButtonPressed(int button) {
		this.wiimote.fireButtonPressedEvent(button);	
	}

	public void simulateButtonReleased(int button) {
		this.wiimote.fireButtonReleasedEvent(button);	
	}

}
